#include "MultiTargetTrackDefine.h"
#include "../comm_define.h"
#include "HardwareInfo.h"
#include <time.h>

#define residual_z_norm_queue_max_count	(30)

TrackInfo::TrackInfo(unsigned int ID, TentativeTrack& trace)
	:UKF(4, 2, 2.0, default_Fx),
	trackID(ID),
	AtSize(0),
	velocity_value(0),
	velocity_heading(0),
	history(0),
	numberOfVisibleFrame(0),
	numberOfConsecutiveInvisible(0),
	residual_z_norm_value_queue(residual_z_norm_queue_max_count),
	velocity_value_queue(10),
	longitude(0),
	latitude(0),
	track_point_seq(2),
	ready_to_report(false){

	timeSecFromEpoch = trace.timeSecFromEpoch;

	UKF.x = trace.UKF.x;
	UKF.P = trace.UKF.P;

	/*
	设置处理过程噪声，主要模型误差由目标加速度引起
	Q_discrete_white_noise
	*/
	double dt = 3.0;
	double acc_var = POW2(0.02);
	MatrixXd QMat = MatrixXd::Zero(4, 4);
	QMat.block<2, 2>(0, 0) = Q_discrete_white_noise(2, dt, acc_var);
	QMat.block<2, 2>(2, 2) = Q_discrete_white_noise(2, dt, acc_var);
	UKF.Q = QMat;


	//设置测量误差
	double R_std_err = 30;
	double A_std_err = 0.3;
	MatrixXd RI = MatrixXd::Zero(2, 2);
	RI(0, 0) = POW2(R_std_err);
	RI(1, 1) = POW2(A_std_err);
	UKF.R = RI;

	type = trace.type;
	ClutterDense = trace.clutterDense;
	//计算合成速度信息
	double vx = UKF.x(1, 0);
	double vy = UKF.x(3, 0);
	calcVelocity(vx, vy, velocity_value, velocity_heading);

	movement_status.history.clear();		
	movement_status.status = NotSure;						
	movement_status.stop_start_tm = 0;							
	movement_status.stop_duration = 0 ;						
	movement_status.move_begin_tm = 0 ;						
	movement_status.move_duration = 0;							
	movement_status.stay_location_lon = 0 ;			
	movement_status.stay_location_lat = 0 ;
	movement_status.stay_location_circle_r = 0 ;

	object_id = NewObjectId(ID);
	creatTimeStamp = time(NULL);

}

TrackInfo::TrackInfo()
:UKF(4, 2, 2.0, default_Fx),
trackID(0),
AtSize(0),
velocity_value(0),
velocity_heading(0),
history(0),
numberOfVisibleFrame(0),
numberOfConsecutiveInvisible(0),
residual_z_norm_value_queue(residual_z_norm_queue_max_count),
velocity_value_queue(10),
longitude(0),
latitude(0),
track_point_seq(2),
ready_to_report(false)
{

}

TrackInfo::~TrackInfo() {
}
double TrackInfo::residual() const {
	return UKF.residual_z_norm_value; 
}

double TrackInfo::residual_mean() const{
	return residual_z_norm_value_queue.mean();
}

bool TrackInfo::is_maneuvering(double* factor) const {
	const double maneuvering_threshold =1.39; //自由度为2的卡方分布0.5分位点
	//double res_norm_mean = residual_z_norm_value_queue.mean(); //使用多次残差的平均值，效果不是很好，会有滞后现象
	double res_norm_mean = UKF.residual_z_norm_value; //使用上一次滤波器更新后的残差值

	bool isMove = movement_status.status == Moveing; //目标是否为移动对象

	if (res_norm_mean > maneuvering_threshold) {
		//如果目标的残差大于阈值，则认为目标进行机动。计算过程误差Q的缩放因子
		if (isMove) {
			/*
			移动目标的缩放因子由2部分组成
			1.残值值
			2.目标速度,速度越大，相应的缩放因子越大
			*/
			if (factor)
				*factor = (res_norm_mean / maneuvering_threshold) * 4.0 + velocity_value * 0.5;
		}
		else {
			/*
			对于静止目标，突然产生较大的残差值，可能是目标从静止突然开始开始移动(地板油起步), 直接使用较大的处理误差
			*/
			if (factor)
				*factor = (res_norm_mean / maneuvering_threshold) * 20.0;
		}
		return true;
	}
	else {
		//目标没有机动
		if (factor)
			*factor = 1.0;
		return false;
	}
}
void TrackInfo::calc_velocity(){
	//这里使用CV模型的速度信息
	double vx = UKF.x(1, 0);
	double vy = UKF.x(3, 0);
	calcVelocity(vx, vy, velocity_value, velocity_heading);
	//velocity_value_queue.push(velocity_value);
	residual_z_norm_value_queue.push(UKF.residual_z_norm_value);
}